// --------------------------------------------------------------------------------------------------------------------
// <copyright file="GlobalMapUtility.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Navigation.Visualization.Localization
{
    using System;
    using System.IO;
    using System.Linq;
    using System.Runtime.Serialization;
    using System.Windows.Media;
    using System.Windows.Media.Imaging;

    using Microsoft.Robotics.IO;
    using Microsoft.Robotics.Navigation.Localization;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;

    /// <summary>
    /// Global Map Utility class which converter global map from image to xml serialized version and vice versa.
    /// </summary>
    public static class GlobalMapUtility
    {
        /// <summary>
        /// Maximum items int the serialized object graph
        /// </summary>
        private const int MaxItemsInObjectGraph = 65535;

        /// <summary>
        /// Converts the given bitmap to global map file
        /// </summary>        
        /// <param name="bitmapFilePath">Path to the bitmap file.</param>
        /// <param name="mapFilePath">Path to the global map to be written.</param>
        /// <param name="resolution">Map resolution in meters per pixel</param>
        /// <param name="mapStartPose">Map start pose relative to the origin</param>
        public static void ConvertBitmapToGlobalMap(string bitmapFilePath, string mapFilePath, float resolution, Pose2D mapStartPose)
        {
            BitmapDecoder uriBitmap = BitmapDecoder.Create(
               new Uri(bitmapFilePath, UriKind.Relative),
               BitmapCreateOptions.PreservePixelFormat,
               BitmapCacheOption.Default);

            BitmapFrame frame = uriBitmap.Frames[0];
            int width = frame.PixelWidth;
            int height = frame.PixelHeight;
            int stride = frame.PixelWidth * PixelFormats.Rgb48.BitsPerPixel / 8;
            if (frame.Format != PixelFormats.Rgb48)
            {
                throw new ArgumentException(string.Format("Bitmap {0} is not Rgb48 image. It is {1} ", bitmapFilePath, frame.Format));
            }

            GlobalMap map = new GlobalMap(width, height, mapStartPose, resolution);
            ushort[] pixels = new ushort[height * stride];
            frame.CopyPixels(pixels, stride, 0);

            int index = 0;
            for (int x = 0; x < height; x++)
            {
                for (int y = 0; y < width; y++, index++)
                {
                    float value = pixels[index * 3] / (float)short.MaxValue - 1;
                    float weight = pixels[index * 3 + 1] / (float)ushort.MaxValue;
                    float metadata = pixels[index * 3 + 2];
                    
                    if (weight != 0)
                    {
                        map.SetCell(x, y, value, weight, metadata);
                    }
                }
            }

            Serializer.Save<GlobalMap>(mapFilePath, map);
        }

        /// <summary>
        /// Converts the given global map file to bitmap
        /// </summary>
        /// <param name="mapFilePath">Path to the global map</param>
        /// <param name="bitmapFilePath">Path to the bitmap file to be written.</param>
        /// <param name="distanceResolution">Distance resolution in meters per pixels</param>
        /// <param name="mapStartPose">Map start pose relative to the origin</param>
        public static void ConvertGlobalMapToBitmap(
            string mapFilePath,
            string bitmapFilePath,
            out float distanceResolution,
            out Pose2D mapStartPose)
        {
            GlobalMap map = null;
            if (mapFilePath != null && System.IO.File.Exists(mapFilePath))
            {
                map = Serializer.Load<GlobalMap>(mapFilePath);
            }

            mapStartPose = map.MapStartPose;
            distanceResolution = map.DistanceResolution;
            ConvertGlobalMapToBitmap(map, bitmapFilePath);
        }

        /// <summary>
        /// Sets global map origin and saves it back
        /// </summary>
        /// <param name="mapFilePath">Path to the global map</param>
        /// <param name="mapOrigin">Robot start point on map</param>
        public static void SetGlobalMapStartPose(
            string mapFilePath,
            Pose2D mapOrigin)
        {
            GlobalMap map = null;
            if (mapFilePath != null && System.IO.File.Exists(mapFilePath))
            {
                map = Serializer.Load<GlobalMap>(mapFilePath);
            }

            map.MapStartPose = mapOrigin;

            Serializer.Save<GlobalMap>(mapFilePath, map);
        }
            
        /// <summary>
        /// Converts the given global map object to a png file. Useful for testing.
        /// </summary>
        /// <param name="map">The global map to save</param>
        /// <param name="bitmapFilePath">Path to the bitmap file to be written.</param>
        public static void ConvertGlobalMapToBitmap(
            GlobalMap map,
            string bitmapFilePath)
        {
            BitmapSource bmp = CreateBitmapFromGlobalMap(map);

            using (FileStream stream = new FileStream(bitmapFilePath, FileMode.Create))
            {
                PngBitmapEncoder encoder = new PngBitmapEncoder();
                encoder.Frames.Add(BitmapFrame.Create(bmp));
                encoder.Save(stream);
            }
        }

        /// <summary>
        /// Creates a bitmap from the given global map file.
        /// </summary>
        /// <param name="map">Global Map file</param>
        /// <returns>Bitmap source of the specified pixel format</returns>
        private static BitmapSource CreateBitmapFromGlobalMap(GlobalMap map)
        {
            int width = map.Width;
            int height = map.Height;
            int stride = map.Width * PixelFormats.Rgb48.BitsPerPixel / 8;

            // a single image has three sections, values first, weights second and metadata third
            ushort[] image = new ushort[width * stride];

            int index = 0;
            for (int x = 0; x < height; x++)
            {
                for (int y = 0; y < width; y++, index++)
                {
                    MapCell cell = map.GetCell(x, y);
                    image[index * 3] = (ushort)((cell.Value + 1) * short.MaxValue);
                    image[index * 3 + 1] = (ushort)((cell.Weight > 1 ? 1 : cell.Weight) * ushort.MaxValue); 
                    image[index * 3 + 2] = (ushort)(cell.Metadata);
                }
            }

            // Creates a new empty image with the pre-defined palette
            BitmapSource bmp = BitmapSource.Create(
                width,
                height,
                96,
                96,
                PixelFormats.Rgb48,
                null,
                image,
                stride);
            return bmp;
        }
    }
}
